/*
  three_Servos
  2022-8-22

  本程序旨在使用Da-Servo库让4个舵机同时运动，通过串口发送A+度数进行控制。
    例如 A90

*/

#include "DaServo.h"
#include <Ticker.h>
Ticker ticker;

#define startPos 0    //舵机往复运动起始位置 / Start position of servo move back and forth
#define endPos 135    //舵机往复运动结束位置 / End position of servo move back and forth
#define servoSpeed 10 //舵机运行速度 / Servo Speed

DaServo myservo1(0, servoSpeed); //建立舵机对象1
DaServo myservo2(1, servoSpeed); //建立舵机对象2
DaServo myservo3(2, servoSpeed); //建立舵机对象3
DaServo myservo4(3, servoSpeed); //建立舵机对象4

// 自定义IIC引脚
static const uint8_t mSDA = 8; // esp32默认为8
static const uint8_t mSCL = 6; // esp32默认为9

void tickerCount()
{
  //重要：servoUpdate函数用于更新舵运行状态，必须经常调用否则舵机无法正常运行!
  // IMPORTANT: servoUpdate MUST be called constantly or servo CANNOT work!
  myservo1.servoUpdate();
  myservo2.servoUpdate();
  myservo3.servoUpdate();
  myservo4.servoUpdate();
}

void setup()
{
  Serial.begin(9600);

  ticker.attach(0.001, tickerCount); //开启定时器

  Wire.setPins(mSDA, mSCL);

  myservo1.init();
  myservo2.init();
  myservo3.init();
  myservo4.init();

  myservo1.setTarget(startPos); //初始设定舵机1运动目标位置 / myservo1 initial target
  myservo2.setTarget(startPos); //初始设定舵机2运动目标位置 / myservo2 initial target
  myservo3.setTarget(startPos); //初始设定舵机3运动目标位置 / myservo3 initial target
  myservo4.setTarget(startPos); //初始设定舵机4运动目标位置 / myservo4 initial target

  // 舵机回零
  while (myservo1.getPosition() != myservo1.getTarget())
  {
    myservo1.servoUpdate();
    myservo2.servoUpdate();
    myservo3.servoUpdate();
    myservo4.servoUpdate();
  }
}

void loop()
{

  if (Serial.available() > 0)
  {
    char serialData = Serial.read();
    if (serialData == 'A')
    {
      int target_servo_angle = Serial.parseInt();
      myservo1.setTarget(target_servo_angle);
      myservo2.setTarget(target_servo_angle);
      myservo3.setTarget(target_servo_angle);
      myservo4.setTarget(target_servo_angle);
    }
  }
}
